/**
 * THIS CLASS NEEDS MORE WORK. DEFINITELY DOES NOT FUNCTION RIGHT NOW.
 */
package edu.wpi.first.wpilibj.defaultCode;

import java.util.TimerTask;

/**
 *
 * @author Wesley
 */
public class EncoderMonitor extends TimerTask
{
    Encoder encoder;
    double[] buf;
    int tail;
    public static int BUF_SIZE = 50;
    private static int NUM_AVG_ENTRIES = 5;


    EncoderMonitor(Encoder encoder)
    {
        this.encoder = encoder;
                buf = new double[BUF_SIZE];
        tail = 0;
    }

    public void run()
    {
        updateRate();
    }

    private void updateRate()
    {
        double val = encoder.getAngle();

        buf[tail] = val;
        tail++;
        if( tail > BUF_SIZE-1 )
            tail = 0;
    }

    public double getRate()
    {
        double avg = 0.0;
        double sum = 0.0;
        int start, end;
        int i;

        if( NUM_AVG_ENTRIES < 1 )
            NUM_AVG_ENTRIES = 1;
        if( NUM_AVG_ENTRIES > BUF_SIZE )
            NUM_AVG_ENTRIES = BUF_SIZE;

        end = tail - 1;
        if( end < 0 )
            end = BUF_SIZE - 1;

        start = end - NUM_AVG_ENTRIES + 1;
        if( start < 0 )
            start = start + BUF_SIZE;

        if( end >= start )
        {
            for(i=start; i<end+1; i++)
                sum = sum + buf[i];
        }
        else
        {
            for( i=start; i<BUF_SIZE; i++ )
                sum = sum + buf[i];
            for( i=0; i<end+1; i++ )
                sum = sum + buf[i];
        }

        avg = sum /(double)NUM_AVG_ENTRIES;

        return avg;
    }
}

